

#include <p32xxxx.h>

#include "tools.h"

#include <string.h>
#include "driverSD_SPI_DMA_ISR_TASK.h"
#include "FreeRTOS.h"
#include "semphr.h"
#include "task.h"
#include "main_peripherals.h"
#include "state.h"
#include "driverLog.h"
#include "remote_TASK.h"
#include "driverRN41_UART_DMA_ISR_TASK.h"
#include "commands.h"

struct sdStatusStruct sdReport;

/**
 SD format:
 - 16 MB header (32768 sectors)
   - 8 sectors (4kb) FAT ident -> "QuadFAT 0x0 0x8 0x9 0x10 .."
   - 128 FAT blocks of 2 sectors (1kb)
     - 4 bytes for level wearing (increments, highest int = last write)
     - 8 bytes per FAT entry -> 0xFFFFFFFF FFFFFFFF = empty, 32 bits start block, 32 bits end block (0xFFFFFFFF when no end)
   - 128 Config blocks of 16 sectors (8kb)
     - 4 bytes for level wearing
 - 4 kb blocks for data...
 */

BYTE sdBuffer[BLOCK_SIZE+16];

BOOL sdValidateFat()
{
    while (sdReadSector(FAT_IDENT_SECTOR,sdBuffer,FAT_IDENT_LENGTH) == 0)
    {
        vTaskDelay(configTICK_RATE_HZ);
    }

    portNOP();

    // "QUADfat\0"
    if (sdBuffer[0] != 0x51 ||
        sdBuffer[1] != 0x55 ||
        sdBuffer[2] != 0x41 ||
        sdBuffer[3] != 0x44 ||
        sdBuffer[4] != 0x66 ||
        sdBuffer[5] != 0x61 ||
        sdBuffer[6] != 0x74 ||
        sdBuffer[7] != 0x00) return FALSE;
    int i;

    for (i=8;i<512*FAT_IDENT_LENGTH;i++)
    {
        if (sdBuffer[i] != (i & 0xFF)) return FALSE;
    }

    unsigned int maxFound = 0;
    int locationFound = 0;
    for (i=0;i<FAT_ENTRIES_WEAR;i++)
    {
        sdReadFat(i);
        unsigned int fatCounter = INT_FROM_BYTES_REV(sdBuffer,0);
        if (fatCounter == 0xFFFFFFFF) continue; // never written before...
        if (fatCounter > maxFound)
        {
            maxFound = fatCounter;
            locationFound = i;
        }
    }
    STATE.sd.currentFATLocation = locationFound;

    sdReadFat(STATE.sd.currentFATLocation);

    for (i=8;i<512 * FAT_ENTRIES_LENGTH;i+=8)
    {
        if (sdBuffer[i] == 0xFF) break;
        if (sdBuffer[i+4] == 0xFF) break;
        int lastBlock = INT_FROM_BYTES_REV(sdBuffer,i+4);
        //STATE.sd.sizeFreeSectors = STATE.sd.sizeSectors - BLOCK_TO_SECTOR(lastBlock) - SECTORS_PER_BLOCK + 1;
        STATE.sdLogging.freeBlocks = STATE.sd.sizeSectors / SECTORS_PER_BLOCK - lastBlock;
    }

    maxFound = 0;
    locationFound = 0;
    for (i=0;i<FAT_CONFIG_WEAR;i++)
    {
        sdReadConfig(i);
        unsigned int configCounter = INT_FROM_BYTES_REV(sdBuffer,0);
        if (configCounter == 0xFFFFFFFF) continue; // Never written before, skip.
        if (configCounter > maxFound)
        {
            maxFound = configCounter;
            locationFound = i;
        }
    }
    STATE.sd.currentConfigLocation = locationFound;

    return TRUE;
}

void sdReadFat(unsigned char fatLocation)
{
    while (sdReadSector(FAT_ENTRIES_SECTOR+fatLocation * FAT_ENTRIES_LENGTH,sdBuffer,FAT_ENTRIES_LENGTH) == 0)
    {
        vTaskDelay(10);
    }
    portNOP();
}

void sdReadConfig(unsigned char configLocation)
{
    while (sdReadSector(FAT_CONFIG_SECTOR + configLocation * FAT_CONFIG_LENGTH, sdBuffer, FAT_CONFIG_LENGTH) == 0)
    {
        vTaskDelay(10);
    }
    portNOP();
}

void sdApplyConfig()
{
    sdReadConfig(STATE.sd.currentConfigLocation);
    if (sdBuffer[4] != 0xFF)
    {
        configReadSD(sdBuffer, 4);
        STATE.system.configLoaded = 1;
    }
}

void sdWriteFat()
{
    unsigned int fatCounter = INT_FROM_BYTES_REV(sdBuffer,0);
    fatCounter++;
    INT_TO_BYTES_REV(fatCounter, sdBuffer, 0);

    STATE.sd.currentFATLocation++;
    if (STATE.sd.currentFATLocation >= FAT_ENTRIES_WEAR) STATE.sd.currentFATLocation = 0;

    sdWriteSector(FAT_ENTRIES_SECTOR + STATE.sd.currentFATLocation * FAT_ENTRIES_LENGTH,sdBuffer,FAT_ENTRIES_LENGTH);
}

void sdWriteConfig()
{
    sdReadConfig(STATE.sd.currentConfigLocation);

    unsigned int fatCounter = INT_FROM_BYTES_REV(sdBuffer,0);
    fatCounter++;
    INT_TO_BYTES_REV(fatCounter, sdBuffer, 0);

    int i;
    for (i=4;i<512*FAT_CONFIG_LENGTH;i++)
        sdBuffer[i] = 0;

    configWriteSD(sdBuffer,4);

    STATE.sd.currentConfigLocation++;
    if (STATE.sd.currentConfigLocation >= FAT_CONFIG_WEAR) STATE.sd.currentConfigLocation = 0;

    if (sdWriteSector(FAT_CONFIG_SECTOR + STATE.sd.currentConfigLocation * FAT_CONFIG_LENGTH,sdBuffer,FAT_CONFIG_LENGTH))
    {
        BUZZ(BUZZ_DELAY_NOTIFY,3);
    }
    else
    {
        BUZZ(BUZZ_DELAY_ERROR,1);
    }
}

void sdRequestStartLog()
{
    extern QueueHandle_t queSD;
    struct SdCommand_t start;
    start.type = SDCMD_START_LOG;
    xQueueSendToBack(queSD,&start,0);
}

void sdRequestEndLog()
{
    extern QueueHandle_t queSD;
    struct SdCommand_t end;
    end.type = SDCMD_END_LOG;
    xQueueSendToBack(queSD,&end,0);
}

void sdStartLog()
{
    if (STATE.sd.fatDetected == 0) return;
    if (STATE.sdLogging.loggingEnabled == 1) return;
    if (STATE.sdLogging.fatEntryId != 0) return;

    sdReadFat(STATE.sd.currentFATLocation);
    
    unsigned int beginBlock = 0;
    int i;
    STATE.sdLogging.fatEntryId = 0;
    for (i=8;i<FAT_ENTRIES_LENGTH*512;i+=8)
    {
        if (sdBuffer[i] == 0xFF || sdBuffer[i+4] == 0xFF)
        {
            STATE.sdLogging.fatEntryId = i/8;
            STATE.sdLogging.logBlock = beginBlock;
            //STATE.sdLogging.freeBlocks = (STATE.sd.sizeSectors - 32768) / 8 - beginBlock;
            INT_TO_BYTES_REV(beginBlock, sdBuffer, i);
            sdBuffer[i+4] = 0xFF;
            sdBuffer[i+5] = 0xFF;
            sdBuffer[i+6] = 0xFF;
            sdBuffer[i+7] = 0xFF;
            sdWriteFat();
            break;
        }
        else
        {
            beginBlock = INT_FROM_BYTES_REV(sdBuffer,i+4) + 1;
        }
    }
    
    if (STATE.sdLogging.fatEntryId == 0)
    {
        // error.
        STATE.sdLogging.loggingEnabled = 0;
    }
    else
    {
        STATE.sdLogging.loggingEnabled = 1;
    }
}

void sdUpdateFAT()
{
    if (STATE.sd.fatDetected == 0) return;
    if (STATE.sdLogging.fatEntryId == 0) return;

    sdReadFat(STATE.sd.currentFATLocation);

    int lastBlock = STATE.sdLogging.logBlock;
    if (lastBlock != INT_FROM_BYTES_REV(sdBuffer, STATE.sdLogging.fatEntryId * 8))
        lastBlock--;

    INT_TO_BYTES_REV(lastBlock, sdBuffer, STATE.sdLogging.fatEntryId * 8 + 4);


    sdWriteFat();
    STATE.sdLogging.shouldUpdateFAT = 0;
}

void sdEndLog()
{
    if (STATE.sd.fatDetected == 0) return;
    if (STATE.sdLogging.loggingEnabled == 0) return;
    if (STATE.sdLogging.fatEntryId == 0) return;

    sdUpdateFAT();

    STATE.sdLogging.loggingEnabled = 0;
}


void sdWriteLog(struct LogBuffer_t * log)
{
    if (STATE.sdLogging.freeBlocks > 0 && STATE.sdLogging.loggingEnabled == 1)
    {
        sdWriteSector(BLOCK_TO_SECTOR(STATE.sdLogging.logBlock),(BYTE*)log->writeBuffer,8);
        STATE.sdLogging.logBlock++;
        STATE.sdLogging.freeBlocks--;
        STATE.sdLogging.shouldUpdateFAT = 1;
    }
    else if (STATE.sdLogging.freeBlocks == 0)
    {
        sdEndLog();
    }

    log->state = LOG_STATE_EMPTY;
}

void sdFatFormat()
{
    int i;
    
    if (STATE.sd.fatDetected)
    {
        // Only clear FAT table
        sdReadFat(STATE.sd.currentFATLocation);
        for (i=4;i<512*FAT_ENTRIES_LENGTH;i++)
        {
            sdBuffer[i] = 0xFF;
        }
        sdWriteFat();

        // Write new config
        sdWriteConfig();
    }
    else
    {
        // Write FAT ident & empty FAT tables

        // QUADfat\0
        sdBuffer[0] = 0x51;
        sdBuffer[1] = 0x55;
        sdBuffer[2] = 0x41;
        sdBuffer[3] = 0x44;
        sdBuffer[4] = 0x66;
        sdBuffer[5] = 0x61;
        sdBuffer[6] = 0x74;
        sdBuffer[7] = 0x00;

        for (i=8;i<512*FAT_IDENT_LENGTH;i++)
            sdBuffer[i] = i & 0xFF;

        sdWriteSector(FAT_IDENT_SECTOR,(BYTE*)sdBuffer,FAT_IDENT_LENGTH);

        for (i=0;i<512*FAT_ENTRIES_LENGTH;i++)
            sdBuffer[i] = 0xFF;
        // Write all fat entries wear...
        for (i = 1; i < FAT_ENTRIES_WEAR;i++)
            sdWriteSector(FAT_ENTRIES_SECTOR+i*FAT_ENTRIES_LENGTH,sdBuffer,FAT_ENTRIES_LENGTH);

        // Write current fat entry...
        sdBuffer[0] = 0x00;
        sdBuffer[1] = 0x00;
        sdBuffer[2] = 0x00;
        sdBuffer[3] = 0x00;
        sdWriteSector(FAT_ENTRIES_SECTOR,sdBuffer,FAT_ENTRIES_LENGTH);


        for (i=0;i<512*FAT_CONFIG_LENGTH;i++)
            sdBuffer[i] = 0x00;
        sdBuffer[0] = sdBuffer[1] = sdBuffer[2] = sdBuffer[3] = 0xFF;

        for (i=1;i < FAT_CONFIG_WEAR;i++)
            sdWriteSector(FAT_CONFIG_SECTOR+i*FAT_CONFIG_LENGTH,sdBuffer,FAT_CONFIG_LENGTH);

        STATE.sd.currentConfigLocation = FAT_CONFIG_WEAR - 1;
        sdWriteConfig();
    }

    if (!sdValidateFat())
    {
        // SHOULD NEVER HAPPEN!
        while(1)
        {
            portNOP();
        }
    }
}

int sdFatFileCount()
{
    sdReadFat(STATE.sd.currentFATLocation);

    int count = 0;
    int i;
    for (i=4;i<FAT_ENTRIES_LENGTH*512;i+=8)
    {
        if (sdBuffer[i] == 0xFF || sdBuffer[i+4] == 0xFF) break;
        count++;
    }

    return count;
}

void sdRawRead(int sector)
{
    sdReadSector(sector, sdBuffer + 5, 1);

    sdBuffer[0] = CMD_SD_RAW_READ;
    INT_TO_BYTES(sector, sdBuffer, 1);

    if (rnClaimTx(50))
    {
        rnTransmitAndConfirmBlock(sdBuffer, 512+5);
        rnReleaseTx();
    }
}

void TaskSD(void * params)
{
    vTaskDelay(configTICK_RATE_HZ / 100);
    BYTE r = sdInitCard();
    if (!r) r = sdInitCard();

    while (!r)
    {
        BUZZ(BUZZ_DELAY_ERROR,2);
        vTaskDelay(configTICK_RATE_HZ*4);
        r = sdInitCard();
    }

    STATE.sd.active = 1;
    
    if (sdValidateFat())
    {
        STATE.sd.fatDetected = 1;
        sdApplyConfig();
    }
    else
    {
        BUZZ(BUZZ_DELAY_ERROR,3);
    }

    struct SdCommand_t cmd;

    while(1)
    {
        extern QueueHandle_t queSD;
        
        if (xQueueReceive(queSD,&cmd,portMAX_DELAY) != pdTRUE) continue;
        STATE.monitoring.driverSDTickCount++;

        if (cmd.type == SDCMD_LOG)
        {
            sdWriteLog((struct LogBuffer_t *)cmd.ref);
        }
        else if (cmd.type == SDCMD_FATFORMAT)
        {
            sdFatFormat();
        }
        else if (cmd.type == SDCMD_START_LOG)
        {
            sdStartLog();
        }
        else if (cmd.type == SDCMD_END_LOG)
        {
            sdEndLog();
        }
        else if (cmd.type == SDCMD_RAW_READ)
        {
            sdRawRead((int)cmd.ref);
        }
        else if (cmd.type == SDCMD_WRITE_CONFIG)
        {
            sdWriteConfig();
        }
        else if (cmd.type == SDCMD_READ_CONFIG)
        {
            sdApplyConfig();
        }
        else if (cmd)

        if (STATE.sdLogging.shouldUpdateFAT >= 32)
        {
            sdUpdateFAT();
            STATE.sdLogging.shouldUpdateFAT = 0;
        }

        vTaskDelay(1);
    }
}

unsigned char sdWriteSpi(unsigned char b)
{
	//SpiChnGetRov(SPI_SD,TRUE);
    SPI1STATCLR = _SPI1STAT_SPIROV_MASK;
    //SpiChnWriteC(SPI_SD,b);
    SPI1BUF = b;

    int timeout = 500;

    while (SPI1STATbits.SPIRBF == 0 && timeout-- > 0) portNOP();
//    while (!SpiChnRxBuffFull(SPI_SD) && timeout-- > 0) portNOP();
    b = SPI1BUF;
    //b = SpiChnReadC(SPI_SD);
	return b;
}

unsigned char sdWaitReady()
{
	SD_CS_LOW;

	int timeout = 0xFFFFFF;
	while ((sdWriteSpi(0xFF) != 0xFF) && timeout > 1);
	{
		portNOP();
		portNOP();
		portNOP();
		portNOP();
		timeout--;
	}

	return timeout > 1 ? 0xFF : 0x00;
}

__inline unsigned char sdWriteReturn(unsigned char r)
{
	SD_CS_HIGH;
	sdWriteSpi(0xFF);
	sdWriteSpi(0xFF);
	sdWriteSpi(0xFF);
	sdWriteSpi(0xFF);
	sdWriteSpi(0xFF);

	return r;
}


unsigned char sdSendCmd(unsigned char cmd, unsigned int arg)
{
	unsigned char timeout;
	unsigned char result;

	/* ACMD<n> is the command sequense of CMD55-CMD<n> */
	if (cmd & 0x80)
	{
		cmd &= 0x7F;
		result = sdSendCmd(CMD55, 0);
		if (result > 1) return result;
		sdWaitReady();
	}

	/* Select the card and wait for ready */
	/*if (sdWaitReady() != 0xFF)
	{
		//sectorsWrittenFailedCmdWait++;
		sdDeselect;
		return 0xFF;
	}*/
	SD_CS_LOW;

	/* Send command packet */
	sdWriteSpi(0x40 | cmd);			/* Start + Command index */
	sdWriteSpi((unsigned char)(arg >> 24));	/* Argument[31..24] */
	sdWriteSpi((unsigned char)(arg >> 16));	/* Argument[23..16] */
	sdWriteSpi((unsigned char)(arg >> 8));		/* Argument[15..8] */
	sdWriteSpi((unsigned char)arg);			/* Argument[7..0] */
	if (cmd == CMD0) sdWriteSpi(0x95);		/* Valid CRC for CMD0(0) */
	else if (cmd == CMD8) sdWriteSpi(0x87);		/* Valid CRC for CMD8(0x1AA) */
	else sdWriteSpi(0x01);	/* Dummy CRC + Stop */

	/* Receive command response */
	if (cmd == CMD12) sdWriteSpi(0xFF);	/* Skip a stuff byte when stop reading */

	timeout = 10;							/* Wait for a valid response in timeout of 10 attempts */
	do
	{
		result = sdWriteSpi(0xFF);
	}
	while (result == 0xFF && timeout-- > 0);

	return result;			/* Return with the response value */
}

/*
 * returns TRUE if successful
 */
unsigned char sdInitCard()
{
    unsigned char i, data[16];
    unsigned char timeout;

	// SPI slow!
    //SpiChnSetBitRate(SPI_SD, configPERIPHERAL_CLOCK_HZ, 250000);
    SPI1BRG = configPERIPHERAL_CLOCK_HZ / 250000 / 2 - 1;
	
    SD_CS_LOW;
	for (i = 0; i < 255; i++) sdWriteSpi(0xFF);
	for (i = 0; i < 255; i++) sdWriteSpi(0xFF);
    sdWriteSpi(0xFD);
    sdWriteSpi(0xFD);
    vTaskDelay(10);
	SD_CS_HIGH;
	/* 80 dummy clockpulses */
	for (i = 0; i < 10; i++) sdWriteSpi(0xFF);

	i = sdSendCmd(CMD0, 0);
	sdWaitReady();

	SD_CS_HIGH;

	if (i != 1) return 0;

	i = sdSendCmd(CMD8, 0x000001AA);

	if (i == 0xFF || i == 0x05) // command not recognized: v1
	{
		//debugLog("SD V1 DETECTED");

		STATE.sd.cardType = 1;
		timeout = 255;

		i = sdSendCmd(ACMD41,0);
		while (i != 0 && --timeout != 0)
		{
			vTaskDelay(1);
			i = sdSendCmd(ACMD41,0);
		}
		SD_CS_HIGH;
		if (timeout == 0) return 0;
	}
	else
	{
		STATE.sd.cardType = 2;
		/* SDv2? */
		//debugLog("SD V2 DETECTED");
		sdWriteSpi(0xFF); // ignore
		sdWriteSpi(0xFF); // ignore
		data[0] = sdWriteSpi(0xFF);
		data[1] = sdWriteSpi(0xFF);

		SD_CS_HIGH;

		if (data[0] != 0x01 || data[1] != 0xAA) return 0;

		//debugWriteString("SD ACMD41\r\n");
		sdWaitReady();
		i = sdSendCmd(ACMD41, 0x40000000);

		SD_CS_HIGH;

		timeout = 255;
		while (i != 0 && timeout != 0)
		{
			vTaskDelay(200);
		//	debugWriteString("SD ACMD41\r\n");
			sdWaitReady();
			i = sdSendCmd(ACMD41, 0x40000000);
			SD_CS_HIGH;
		}


		if (i != 0) return 0;

		sdWaitReady();
		i = sdSendCmd(CMD58, 0);

		data[0] = sdWriteSpi(0xFF);
		data[1] = sdWriteSpi(0xFF);
		data[2] = sdWriteSpi(0xFF);
		data[3] = sdWriteSpi(0xFF);

		if (((data[0] & 0x40) == 0x40) && (data[0] != 0xFF))
		{
			STATE.sd.cardHighCapacity = 1;
		}
		else
		{
			STATE.sd.cardHighCapacity = 0;
		}
	}

	SD_CS_HIGH;
	sdWaitReady();
	i = sdSendCmd(CMD9, 0);
	if (i != 0) return 0;

	while (sdWriteSpi(0xFF) != 0xFE); // wait for start

	for (i=0;i<16;i++) data[i] = sdWriteSpi(0xFF); // read 16 CSD bytes

	sdWriteSpi(0xFF);
	sdWriteSpi(0xFF);
	SD_CS_HIGH;

	sdWaitReady();
	SD_CS_HIGH;

	if (STATE.sd.cardHighCapacity == 1)
	{            // Uses the SDHC capacity calculation
		STATE.sd.sizeKBytes = data[9] + 1;
		STATE.sd.sizeKBytes += (unsigned int)(data[8] << 8);
		STATE.sd.sizeKBytes += (unsigned int)(data[7] & 0x0F)<<12;
		STATE.sd.sizeKBytes *= 512;                // multiply by 512kb / 1024

		// (C_SIZE + 1) * 512 * 1024
		STATE.sd.sizeSectors = STATE.sd.sizeKBytes*2;
	}
	else
	{                                // Uses the SD capacity calculation
		STATE.sd.sizeKBytes = (((data[6] & 0x03)<<10) | (data[7]<<2) | ((data[8] & 0xC0)>>6)) + 1;
		STATE.sd.sizeKBytes = STATE.sd.sizeKBytes <<((((data[9] & 0x03)<<1) | ((data[10] & 0x80)>>7)) +2);
		STATE.sd.sizeKBytes /= 1024;
		STATE.sd.sizeKBytes = STATE.sd.sizeKBytes <<((data[5] & 0x0F));
		// (C_SIZE +1) <<(C_SIZE_MULT + 2) <<(READ_BL_LEN), then set SET_BLOCKLEN to be 512 next.
		STATE.sd.sizeSectors = STATE.sd.sizeKBytes*2;
		sdWaitReady;
	}

	i = sdSendCmd(CMD16,0x00000200);        // Set block size to 512 bytes
	SD_CS_HIGH;
	sdWaitReady;
	SD_CS_HIGH;

	/* SPI high speed */

    SPI1CONCLR = _SPI1CON_ON_MASK;

	//SpiChnEnable(SPI_SD,0);
	SPI1BRG = 0; // 0 = 40mz @ 80fpb, 0 = 38mhz @ 76 fpb, 1 = 19mhz @ 76 fpb, 2 = 12.6mhz @ 76 fpb
	//SpiChnEnable(SPI_SD,1);

    SPI1CONSET = _SPI1CON_ON_MASK;

	return 1;
}

/*
 *  returns  TRUE if successful
 */
unsigned char sdReadSector(unsigned int sector, unsigned char * p, unsigned char sectorCount)
{
    unsigned char r;
	int currentSector = 0;

    if (STATE.sd.cardHighCapacity == 0) sector <<= 9;

    // 1. send READ command
    if (sectorCount == 1) r = sdSendCmd(CMD17, sector); // single block read
    else r = sdSendCmd(CMD18, sector); // multiple block read

    if (r != 0)
    {
        SD_CS_HIGH;
        portNOP();
        return sdWriteReturn(0);
    }
    

    SD_CS_LOW;
    while (currentSector++ < sectorCount)
    {
        // 2. wait for a response
		for(;;)
		{
			r = sdWriteSpi(0xFF);
			if (r == 0xFE) break;
		}

		// read 512 byte of data
        sdStartDMARx(p);
        extern SemaphoreHandle_t semSDTX;
		if (xSemaphoreTake(semSDTX,50) != pdPASS)
		{
            sdSendCmd(CMD12,0);
            SD_CS_HIGH;
			return sdWriteReturn(0);
		}

		// 4. ignore CRC
		sdWriteSpi(0xFF);
		sdWriteSpi(0xFF);
        p += 512;
    }// 5. remember to disable the card
    
    if (sectorCount > 1)
    {
        sdSendCmd(CMD12,0);
    }

	sdWaitReady();
	SD_CS_HIGH;

    return (r == 0xFE);    // return TRUE if successful
} // readSECTOR

void sdStartDMATx(unsigned char * buff)
{
    IFS0CLR = _IFS0_SPI1TXIF_MASK;
	//INTClearFlag(INT_SOURCE_SPI_TX(SPI_SD));

    //INTClearFlag(INT_SOURCE_DMA(DMA_SD_TX));		// clear the existing channel interrupt flags
    IFS1CLR = _IFS1_DMA5IF_MASK;

    // Block transfer complete ISR
    DCH5INTSET = _DCH5INT_CHBCIE_MASK;
    DCH5INTCLR = _DCH5INT_CHBCIF_MASK;

	//DmaChnSetTxfer(DMA_SD_TX,buff, (void*)&SPI1BUF, 512, 1, 1);
    DCH5SSA = KVA_TO_PA(buff); //source address
    DCH5SSIZ = 512; //source size
    DCH5SPTR = 0; //source pointer; TODO: check if needed

    DCH5DSA = KVA_TO_PA(&SPI1BUF); //dest address
    DCH5DSIZ = 1; //dest size
    DCH5DPTR = 0; //dest pointer; TODO: check if needed

    DCH5CSIZ = 1; //cell size
    DCH5CPTR = 0; //cell pointer; TODO: check if needed

	//DmaChnSetEvEnableFlags(DMA_SD_TX, DMA_EV_BLOCK_DONE);

    IEC1SET = _IEC1_DMA5IE_MASK;
	//INTEnable(INT_SOURCE_DMA(DMA_SD_TX), INT_ENABLED);	 // enable the chn interrupt in the INT controller

	//DmaChnStartTxfer(DMA_SD_TX, DMA_WAIT_NOT, 0);

    // Enable channel
    DCH5CONSET = _DCH5CON_CHEN_MASK;
    DCH5ECONSET = _DCH5ECON_CFORCE_MASK;
}

void sdStartDMARx(unsigned char * buff)
{
    IFS1CLR = _IFS1_DMA5IF_MASK | _IFS1_DMA0IF_MASK;
    IFS0CLR = _IFS0_SPI1TXIF_MASK | _IFS0_SPI1RXIF_MASK;

//	INTClearFlag(INT_SOURCE_DMA(DMA_SD_TX));
//	INTClearFlag(INT_SOURCE_DMA(DMA_SD_RX));
//	INTClearFlag(INT_SOURCE_SPI_TX(SPI_SD));
//	INTClearFlag(INT_SOURCE_SPI_RX(SPI_SD));


    //DmaChnSetTxfer(DMA_SD_RX, (void*)&SPI1BUF, buff, 1, 512, 1);
    {
        DCH0SSA = KVA_TO_PA(&SPI1BUF); //source address
        DCH0SSIZ = 1; //source size
        DCH0SPTR = 0; //source pointer; TODO: check if needed

        DCH0DSA = KVA_TO_PA(buff); //dest address
        DCH0DSIZ = 512; //dest size
        DCH0DPTR = 0; //dest pointer; TODO: check if needed

        DCH0CSIZ = 1; //cell size
        DCH0CPTR = 0; //cell pointer; TODO: check if needed        
    }



    //DmaChnSetTxfer(DMA_SD_TX,buff, (void*)&SPI1BUF, 512, 1, 1);
    {
        DCH5SSA = KVA_TO_PA(buff); //source address
        DCH5SSIZ = 512; //source size
        DCH5SPTR = 0; //source pointer; TODO: check if needed

        DCH5DSA = KVA_TO_PA(&SPI1BUF); //dest address
        DCH5DSIZ = 1; //dest size
        DCH5DPTR = 0; //dest pointer; TODO: check if needed

        DCH5CSIZ = 1; //cell size
        DCH5CPTR = 0; //cell pointer; TODO: check if needed
    }


    DCH0INT = _DCH0INT_CHBCIE_MASK | _DCH0INT_CHERIE_MASK;
    DCH5INT = _DCH0INT_CHERIE_MASK; //_DCH0INT_CHBCIE_MASK | _DCH0INT_CHERIE_MASK;

    SPI1CONSET = _SPI1CON_DISSDO_MASK; // disable SDO pin & set high (fake 0xFF)
    SD_DO_HIGH;

	//SpiChnReadC(SPI_SD);
    SPI1BUF;

    SPI1STATCLR = _SPI1STAT_SPIROV_MASK;

    IEC1SET = _IEC1_DMA5IE_MASK | _IEC1_DMA0IE_MASK;
//	INTEnable(INT_SOURCE_DMA(DMA_SD_RX), INT_ENABLED);
//	INTEnable(INT_SOURCE_DMA(DMA_SD_TX), INT_ENABLED);
    DCH0CONSET = _DCH0CON_CHEN_MASK;
    DCH5CONSET = _DCH5CON_CHEN_MASK;

    DCH5ECONSET = _DCH5ECON_CFORCE_MASK;

    //DmaChnForceTxfer(DMA_SD_TX);
}

unsigned char sdWriteSector(unsigned int sector, unsigned char * buff, unsigned char sectorCount)
{
	// claim SPI bus
    unsigned int writeTimeout;
	unsigned char r;
	unsigned char sectorsWritten;
	sectorsWritten = 0;

    STATE.sd.sizeWrittenSectors += sectorCount;
    
    sdWaitReady();

    if (STATE.sd.cardType & CT_SDC)
    {
        r = sdSendCmd(ACMD23, sectorCount);
    }

    if (STATE.sd.cardHighCapacity == 0) sector <<= 9;
	r = sdSendCmd(sectorCount == 1 ? CMD24 : CMD25, sector); //sector << 9);

	// if command accepted
	if (r != 0)
	{
		vTaskDelay(1000);
		sdReport.writeFailCmd++;
		return sdWriteReturn(0);
	}

	r = sdWaitReady();
	if (r != 0xFF)
	{
		r = 0;
		return sdWriteReturn(r);
	}

	while (sectorsWritten++ < sectorCount)
	{
		sdWriteSpi(sectorCount == 1 ? 0xFE : 0xFC);			// send data start

		sdStartDMATx(buff);									// start DMA transfer

		// wait for DMA IRQ
        extern SemaphoreHandle_t semSDTX;
		if (xSemaphoreTake(semSDTX,100) != pdPASS)
		{
            IEC1CLR = _IEC1_DMA5IE_MASK;
			return sdWriteReturn(0);
		}

        IEC1CLR = _IEC1_DMA5IE_MASK;

		//INTEnable(INT_SOURCE_DMA(DMA_SD_TX), INT_DISABLED);

		while (SPI1STATbits.SPIBUSY==1);

        // 2 char dummy crc's
		if (sectorCount == 1)
		{
			sdWriteSpi(0xFF);
			sdWriteSpi(0xFF);
		}
        
		writeTimeout = 520;

		do {
			r = sdWriteSpi(0xFF);
        } while (r == 0xFF && writeTimeout-- > 0);

        // accepted
        if ((r & 0x1f) == 0x05)
        {
            r = 1;
            sdReport.writeSuccess++;
        }

        else
        {
            r = 0;
            sdReport.writeFailAccepted++;
        }

        sdWaitReady();
        portNOP();
        buff += 512;
	}

	if (sectorCount != 1)
	{
		sdWriteSpi(0xFD);
	}

	sdWaitReady();

	return sdWriteReturn(r);
}

void __attribute__((vector(_DMA_5_VECTOR), interrupt(ipl5))) ISRWRAPPER_SD_DMA_TX();

void ISR_SD_DMA_TX(void)
{
	static portBASE_TYPE higherPriorTask;
	higherPriorTask = 0;

    IFS1CLR = _IFS1_DMA5IF_MASK;
	//INTClearFlag(INT_SOURCE_DMA(DMA_SD_TX));

	//int evFlags = DmaChnGetEvFlags(DMA_SD_TX);	// get the event flags

//    if (evFlags & DMA_EV_ERR)
    if (DCH5INTbits.CHERIF)
    {
        portNOP();
        DCH5INTCLR = _DCH5INT_CHERIF_MASK;
//        DmaChnClrEvFlags(DMA_SD_TX, DMA_EV_ERR);
    }
//    if (evFlags & DMA_EV_BLOCK_DONE)
    if (DCH5INTbits.CHBCIF)
    {
        SPI1STATCLR = _SPI1STAT_SPIROV_MASK;
        extern SemaphoreHandle_t semSDTX;
    	xSemaphoreGiveFromISR(semSDTX,&higherPriorTask);
//		DmaChnClrEvFlags(DMA_SD_TX, DMA_EV_BLOCK_DONE);
        DCH5INTCLR = _DCH5INT_CHBCIF_MASK;
    }
	else
	{
		portNOP();
	}

	portEND_SWITCHING_ISR(higherPriorTask);
}


void __attribute__((vector(_DMA_0_VECTOR), interrupt(ipl5))) ISRWRAPPER_SD_DMA_RX();

void ISR_SD_DMA_RX(void)
{
	static portBASE_TYPE higherPriorTask;
	higherPriorTask = 0;

    IFS1CLR = _IFS1_DMA0IF_MASK;
	//INTClearFlag(INT_SOURCE_DMA(DMA_SD_RX));

//	int evFlags = DmaChnGetEvFlags(DMA_SD_RX);	// get the event flags

//    if (evFlags & DMA_EV_ERR)
    if (DCH0INTbits.CHERIF)
    {
        //DmaChnClrEvFlags(DMA_SD_RX, DMA_EV_ERR);
        DCH0INTCLR = _DCH0INT_CHERIF_MASK;
        portNOP();
    }

    //if (evFlags & DMA_EV_BLOCK_DONE)
    if (DCH0INTbits.CHBCIF)
    {
        SPI1STATCLR = _SPI1STAT_SPIROV_MASK;
		//SpiChnReadC(SPI_SD);
        SPI1BUF;

        extern SemaphoreHandle_t semSDTX;
    	xSemaphoreGiveFromISR(semSDTX,&higherPriorTask);
        SPI1CONCLR = _SPI1CON_DISSDO_MASK;
//        DmaChnClrEvFlags(DMA_SD_TX, DMA_EV_BLOCK_DONE);
        DCH0INTCLR = _DCH0INT_CHBCIF_MASK;
    }

	portEND_SWITCHING_ISR(higherPriorTask);
}

